#include "car_motion.h"


#include "stdio.h"
#include "math.h"
#include "mcnamara.h"

#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include "motor.h"

#ifndef M_PI
#define M_PI 3.141592653589793f
#endif

car_motion_t micro_car;



// 小车停止 Car stop
void Motion_Stop(uint8_t brake)
{
    Motor_Stop(brake);
}

// 控制小车运动
// Control car motion
// Control car motion
void Motion_Ctrl(float V_x, float V_y, float V_z)
{
    // 完全适配ROS2 cmd_vel坐标系：x为前进，y为左移
    float speed = sqrtf(V_x * V_x + V_y * V_y); // 0~1
    float deflection = atan2f(V_y, V_x) * 180.0f / M_PI;
    if (deflection < 0) deflection += 360.0f;
    float rate = -V_z;
    float l1, l2, r1, r2;
    set_deflection_rate(speed, deflection, rate, &l1, &l2, &r1, &r2);
    Motor_Set_Speed(l1, l2, r1, r2);
}


// 获取小车运动的速度
// Get the speed of the car's motion
void Motion_Get_Speed(car_motion_t* car)
{
    float speed_m1 = 0, speed_m2 = 0, speed_m3 = 0, speed_m4 = 0;
    Motor_Get_Speed(&speed_m1, &speed_m2, &speed_m3, &speed_m4);

    // 麦克纳姆轮标准解算，轮序：M1(左前), M2(左后), M3(右前), M4(右后)
    car->Vx = (speed_m1 + speed_m2 + speed_m3 + speed_m4) / 4.0f; // 前进为正
    car->Vy = (-speed_m1 + speed_m2 + speed_m3 - speed_m4) / 4.0f; // 左移为正
    car->Wz = (-speed_m1 + speed_m2 - speed_m3 + speed_m4) / (2.0f * (0.117f + 0.132f)); // 轮距+轴距，单位m
}

// 控制小车的运动状态
// Control the motion state of the car
void Motion_Ctrl_State(uint8_t state, float speed)
{
    if (speed < 0) speed = -speed;
    if (speed > 1.0) speed = 1.0;
    switch (state)
    {
    case MOTION_STOP:
        Motion_Stop(STOP_COAST);
        break;
    case MOTION_RUN:
        Motion_Ctrl(speed, 0, 0);
        break;
    case MOTION_BACK:
        Motion_Ctrl(-speed, 0, 0);
        break;
    case MOTION_LEFT:
        Motion_Ctrl(speed, 0, speed*ROBOT_SPIN_SCALE);
        break;
    case MOTION_RIGHT:
        Motion_Ctrl(speed, 0, -speed*ROBOT_SPIN_SCALE);
        break;
    case MOTION_SPIN_LEFT:
        Motion_Ctrl(0, 0, speed*ROBOT_SPIN_SCALE);
        break;
    case MOTION_SPIN_RIGHT:
        Motion_Ctrl(0, 0, -speed*ROBOT_SPIN_SCALE);
        break;
    case MOTION_BRAKE:
        Motion_Stop(STOP_BRAKE);
        break;
    default:
        break;
    }
}

void Motion_Init(void)
{
    Motor_Init();
}